#include <cmath>
#include <graph_search.h>

using namespace JPS;
GraphSearch::GraphSearch(const cv::Mat &cMap, int xDim, int yDim, double eps, bool verbose) :
  cMap_(cMap), xDim_(xDim), yDim_(yDim), eps_(eps), verbose_(verbose)
{
  hm_.resize(xDim_ * yDim_);
  seen_.resize(xDim_ * yDim_, false);

  for(int x = -1; x <= 1; x ++) {
    for(int y = -1; y <= 1; y ++) {
      if (x == 0 && y == 0)
        continue;
      ns_.push_back(std::vector<int>{x, y});
    }
  }

  jn2d_ = std::make_shared<JPS2DNeib>();
}

inline int GraphSearch::coordToId(int x, int y) const {
  return x + y * xDim_;
}

inline bool GraphSearch::isFree(int x, int y) const {
  return x >= 0 && x < xDim_ && y >= 0 && y < yDim_ && (static_cast<int>(cMap_.at<uchar>(y, x)) == 255);
}

inline bool GraphSearch::isOccupied(int x, int y) const {
  return x >= 0 && x < xDim_ && y >= 0 && y < yDim_ && (static_cast<int>(cMap_.at<uchar>(y, x)) != 255);
}

inline double GraphSearch::getHeur(int x, int y) const {
  return eps_ * std::sqrt((x - xGoal_) * (x - xGoal_) + (y - yGoal_) * (y - yGoal_));
  // return eps_ * ((x - xGoal_) * (x - xGoal_) + (y - yGoal_) * (y - yGoal_));
}

bool GraphSearch::plan(std::pair<int, int> xyStart, std::pair<int, int> xyGoal, bool useJps, int maxExpand)
{
  use_2d_ = true;
  pq_.clear();        // 清空优先级队列
  path_.clear();      // 清空路径
  hm_.resize(xDim_ * yDim_);
  seen_.resize(xDim_ * yDim_, false); // 闭区间

  use_jps_ = useJps;

  int goal_id = coordToId(xyGoal.first, xyGoal.second);
  xGoal_ = xyGoal.first;
  yGoal_ = xyGoal.second;

  int start_id = coordToId(xyStart.first, xyStart.second);
  StatePtr currNode_ptr = std::make_shared<State>(State(start_id, xyStart.first, xyStart.second, 0, 0));
  currNode_ptr->g = 0;
  currNode_ptr->h = getHeur(xyStart.first, xyStart.second);

  return plan(currNode_ptr, maxExpand, start_id, goal_id);
}

bool GraphSearch::plan(StatePtr& currNode_ptr, int maxExpand, int start_id, int goal_id) 
{
  currNode_ptr->heapkey = pq_.push(currNode_ptr);
  currNode_ptr->opened = true;
  hm_[currNode_ptr->id] = currNode_ptr;
  seen_[currNode_ptr->id] = true;

  int expand_iteration = 0;
  while(true)
  {
    expand_iteration++;
    currNode_ptr = pq_.top(); 
    pq_.pop();
    currNode_ptr->closed = true; // 添加到关闭列表

    // 判断当前节点是否击中目标节点
    if(currNode_ptr->id == goal_id) {
      if(verbose_)
        printf("Goal Reached!!!!!!\n\n");
      break;
    }

    std::vector<int> succ_ids;
    std::vector<double> succ_costs;
    if(!use_jps_)
      getSucc(currNode_ptr, succ_ids, succ_costs);
    else
      getJpsSucc(currNode_ptr, succ_ids, succ_costs);

    for (int s = 0; s < (int)succ_ids.size(); s++)
    {
      StatePtr& child_ptr = hm_[succ_ids[s]];
      double tentative_gval = currNode_ptr->g + succ_costs[s];  // 计算新节点的成本代价值

      // 判断父节点代价值 加上 当前节点代价值的和 是否 小于 之前的代价值 (节点的代价值都是初始为无穷大)
      if (tentative_gval < child_ptr->g)
      {
        child_ptr->parentId = currNode_ptr->id;  // 更新父亲节点
        child_ptr->g = tentative_gval;           // 更新成本代价值

        if (child_ptr->opened && !child_ptr->closed){
          pq_.increase(child_ptr->heapkey); // update heap
          child_ptr->dx = (child_ptr->x - currNode_ptr->x);
          child_ptr->dy = (child_ptr->y - currNode_ptr->y);
          child_ptr->dz = (child_ptr->z - currNode_ptr->z);

          if (child_ptr->dx != 0)
            child_ptr->dx /= std::abs(child_ptr->dx);
          if (child_ptr->dy != 0)
            child_ptr->dy /= std::abs(child_ptr->dy);
          if (child_ptr->dz != 0)
            child_ptr->dz /= std::abs(child_ptr->dz);
        }
        else if (child_ptr->opened && child_ptr->closed){
          printf("ASTAR ERROR!\n");
        }
        else{
          child_ptr->heapkey = pq_.push(child_ptr);
          child_ptr->opened = true;
        }
      }
    }

    if(maxExpand > 0 && expand_iteration >= maxExpand) {
      if(verbose_)
        printf("MaxExpandStep [%d] Reached!!!!!!\n\n", maxExpand);
      return false;
    }

    if (pq_.empty()){
      if (verbose_)
        printf("Priority queue is empty!!!!!!\n\n");
      return false;
    }
  }

  if(verbose_) {
    printf("goal g: %f, h: %f!\n", currNode_ptr->g, currNode_ptr->h);
    printf("Expand [%d] nodes!\n", expand_iteration);
  }

  path_ = recoverPath(currNode_ptr, start_id);
  return true;
}


std::vector<StatePtr> GraphSearch::recoverPath(StatePtr node, int start_id) {
  std::vector<StatePtr> path;
  path.push_back(node);
  while(node && node->id != start_id) {
    node = hm_[node->parentId];
    path.push_back(node);
  }

  return path;
}

void GraphSearch::getSucc(const StatePtr& curr, std::vector<int>& succ_ids, std::vector<double>& succ_costs) {
  if(use_2d_) {
    for(const auto& d: ns_) {
      int new_x = curr->x + d[0];
      int new_y = curr->y + d[1];
      if(!isFree(new_x, new_y))
        continue;

      int new_id = coordToId(new_x, new_y);
      if(!seen_[new_id]) {
        seen_[new_id] = true;
        hm_[new_id] = std::make_shared<State>(new_id, new_x, new_y, d[0], d[1]);
        hm_[new_id]->h = getHeur(new_x, new_y);
      }

      succ_ids.push_back(new_id);
      succ_costs.push_back(std::sqrt(d[0]*d[0]+d[1]*d[1]));
    }
  }
}

// 输出: 符合条件的跳点 和 成本代价值
void GraphSearch::getJpsSucc(const StatePtr& curr, std::vector<int>& succ_ids, std::vector<double>& succ_costs) 
{
  if(use_2d_) {
    const int norm1 = std::abs(curr->dx) + std::abs(curr->dy); // 根据父节点与当前节点的方向来找到当前节点的邻居节点 和 强制邻居节点
    int num_neib = jn2d_->nsz[norm1][0];                       // 找到当前节点的邻居节点个数 8、1、3
    int num_fneib = jn2d_->nsz[norm1][1];                      // 找到当前节点需要判断障碍物节点的个数 0、2、2

    int id = (curr->dx + 1) + 3 * (curr->dy + 1); // 转换为一维栅格坐标

    // 分别进行遍历: 邻居节点、强制邻居节点
    for (int dev = 0; dev < num_neib + num_fneib; ++dev){
      int new_x, new_y;
      int dx, dy;
      if(dev < num_neib) {
        dx = jn2d_->ns[id][0][dev];
        dy = jn2d_->ns[id][1][dev];
        if (!jump(curr->x, curr->y, dx, dy, new_x, new_y))
          continue;
      }
      else {
        int nx = curr->x + jn2d_->f1[id][0][dev - num_neib];
        int ny = curr->y + jn2d_->f1[id][1][dev - num_neib];
        if(isOccupied(nx,ny)) {
          dx = jn2d_->f2[id][0][dev - num_neib];
          dy = jn2d_->f2[id][1][dev - num_neib];
          if (!jump(curr->x, curr->y, dx, dy, new_x, new_y))
            continue;
        }
        else
          continue;
      }

      // 将符合条件的坐标栅格转换成一维栅格坐标 并 判断是否在闭合区间内
      int new_id = coordToId(new_x, new_y);
      if(!seen_[new_id]) {
        seen_[new_id] = true;

        hm_[new_id] = std::make_shared<State>(new_id, new_x, new_y, dx, dy);
        hm_[new_id]->h = getHeur(new_x, new_y); // 创建新节点并更新它的启发函数值
      }

      succ_ids.push_back(new_id);
      succ_costs.push_back(std::sqrt((new_x - curr->x) * (new_x - curr->x) + (new_y - curr->y) * (new_y - curr->y)));
      // succ_costs.push_back((new_x - curr->x) * (new_x - curr->x) + (new_y - curr->y) * (new_y - curr->y));
    }
  }
}

// 跳点函数
// 输入: 当前父亲节点坐标 和 方向坐标
// 1、得到当前节点的坐标
// 2、依次判断当前节点是否为障碍物点或超出边界、是否击中目标点、是否存在强制邻居节点
// 3、寻找当前节点的邻居节点进行递归操作
// 4、沿着当前节点的前进方向继续寻找
bool GraphSearch::jump(int x, int y, int dx, int dy, int &new_x, int &new_y){
  new_x = x + dx;
  new_y = y + dy;

  // 超出边界或是障碍物点则返回 false
  if (!isFree(new_x, new_y))
    return false;

  // 击中目标点 返回 true
  if (new_x ==  xGoal_ && new_y == yGoal_)
    return true;

  // 判断是否存在强制邻居节点
  if (hasForced(new_x, new_y, dx, dy))
    return true;

  const int id = (dx + 1) + 3 * (dy + 1);
  const int norm1 = std::abs(dx) + std::abs(dy);
  int num_neib = jn2d_->nsz[norm1][0];  // 找到当前节点的邻居点个数

  // 首先遍历自己的邻居节点进行判读
  // 邻居节点再去找到自己的邻居节点进行判断
  for (int k = 0; k < num_neib - 1; ++k){
    int new_new_x, new_new_y;
    if (jump(new_x, new_y, jn2d_->ns[id][0][k], jn2d_->ns[id][1][k], new_new_x, new_new_y))
      return true;
  }

  return jump(new_x, new_y, dx, dy, new_x, new_y);
}

// 输入: 当前节点坐标 和 方向
// 输出: 判断需要判断障碍物栅格节点的坐标是否为障碍物点
inline bool GraphSearch::hasForced(int x, int y, int dx, int dy) {
  const int id = (dx + 1) + 3 * (dy + 1);
  for (int fn = 0; fn < 2; ++fn){
    int nx = x + jn2d_->f1[id][0][fn];
    int ny = y + jn2d_->f1[id][1][fn];
    if (isOccupied(nx, ny))
      return true;
  }
  return false;
}

std::vector<StatePtr> GraphSearch::getPath() const {
  return path_;
}

std::vector<StatePtr> GraphSearch::getOpenSet() const {
  std::vector<StatePtr> ss;
  for(const auto& it: hm_) {
    if(it && it->opened && !it->closed)
      ss.push_back(it);
  }
  return ss;
}

std::vector<StatePtr> GraphSearch::getCloseSet() const {
  std::vector<StatePtr> ss;
  for(const auto& it: hm_) {
    if(it && it->closed)
      ss.push_back(it);
  }
  return ss;
}

std::vector<StatePtr> GraphSearch::getAllSet() const {
  std::vector<StatePtr> ss;
  for(const auto& it: hm_) {
    if(it)
      ss.push_back(it);
  }
  return ss;
}

constexpr int JPS2DNeib::nsz[3][2];

JPS2DNeib::JPS2DNeib() {
  int id = 0;
  for (int dy = -1; dy <= 1; ++dy)
  {
    for (int dx = -1; dx <= 1; ++dx)
    {
      int norm1 = std::abs(dx) + std::abs(dy);
      for (int dev = 0; dev < nsz[norm1][0]; ++dev)
        Neib(dx, dy, norm1, dev, ns[id][0][dev], ns[id][1][dev]);

      // norm1 : 0、1、2
      //   dev : 0、2、2
      for (int dev = 0; dev < nsz[norm1][1]; ++dev){
        FNeib(dx, dy, norm1, dev, f1[id][0][dev], f1[id][1][dev], f2[id][0][dev], f2[id][1][dev]);
      }
      id++;
    }
  }
}

void JPS2DNeib::print() {
  for(int dx = -1; dx <= 1; dx++) {
    for(int dy = -1; dy <= 1; dy++) {
      int id = (dx + 1) + 3 * (dy + 1);
      printf("[dx: %d, dy: %d]-->id: %d:\n", dx, dy, id);
      for (unsigned int i = 0; i < sizeof(f1[id][0]) / sizeof(f1[id][0][0]); i++)
        printf("  f1: [%d, %d]\n", f1[id][0][i], f1[id][1][i]);
    }
  }
}

void JPS2DNeib::Neib(int dx, int dy, int norm1, int dev, int& tx, int& ty)
{
  switch(norm1)
  {
    // 8个方向
    case 0:
      switch(dev){
        case 0: tx = 1; ty = 0; return;
        case 1: tx = -1; ty = 0; return;
        case 2: tx = 0; ty = 1; return;
        case 3: tx = 1; ty = 1; return;
        case 4: tx = -1; ty = 1; return;
        case 5: tx = 0; ty = -1; return;
        case 6: tx = 1; ty = -1; return;
        case 7: tx = -1; ty = -1; return;
     }
    // 4个方向
    case 1:
      tx = dx; ty = dy; return;
    // 3个方向
    case 2:
      switch(dev){
        case 0: tx = dx; ty = 0; return;
        case 1: tx = 0; ty = dy; return;
        case 2: tx = dx; ty = dy; return;
      }
  }
}

// f1 为寻找障碍物坐标
// f2 为寻找强制邻居坐标
void JPS2DNeib::FNeib(int dx, int dy, int norm1, int dev, int &fx, int &fy, int &nx, int &ny)
{
  switch(norm1)
  {
    case 1:
      switch(dev){
      case 0:
        fx = 0; fy = 1;
        break;
      case 1:
        fx = 0; fy = -1;
        break;
      }
      // 水平或垂直方向上
      // 以当前的 X 栅格为中心，向 该栅格的水平或垂直方向上寻找
      // (如果是上下的方向，则需要寻找左右两边的邻居是否为障碍物) (如果是左右的方向，则需要寻找上下两边的邻居是否为障碍物)
      if(dx == 0)
        fx = fy, fy = 0;
      
      nx = dx + fx;
      ny = dy + fy;
      return;

    case 2:
      switch (dev){

      // 斜向上的节点、也是存在障碍物邻居判断 和 强制邻居
      // 以当前的 X 栅格为中心，向 该栅格的斜向方向寻找
      case 0:
        fx = -dx; fy = 0;
        nx = -dx; ny = dy;
        return;
      case 1:
        fx = 0;  fy = -dy;
        nx = dx; ny = -dy;
        return;
      }
  }
}





